package prawnBot;

import lejos.nxt.*;

/**
 * Main program execution of the design project. Controls main system logic
 * and calls all appropriate subroutines. Runs indefinitely, or until the
 * task is completed.
 * 
 * @author Stephane Beniak - 260324952
 */
public class Main {
   
   /** Defines the current amount of blocks in the stacking area. */
   public static int stackCount = 0;
   /** Defines whether or not the robot is holding a pallet. True = yes. */
   public static boolean pallet = false;
   
   /** Defines the length, in cm, of one foot (one gridline). */
   public static final double FOOT = 30.48;
   
   /**
    * Main method of the program which controls program flow.
    * 
    * @param args Command line arguments - not used.
    */
   public static void main (String[] args) {
      
      // Setup the odometer, display, ultrasonic sensors, lightsensors, localizer, gridsnapper, and block finder.
      TwoWheeledRobot robot = new TwoWheeledRobot(Motor.A, Motor.B);
      Odometer odo = new Odometer(robot, true);
      USPoller usp = new USPoller(new UltrasonicSensor(SensorPort.S1), new UltrasonicSensor(SensorPort.S2), 70);
      LightSensor lsL = new LightSensor(SensorPort.S3);
      LightSensor lsR = new LightSensor(SensorPort.S4);
      LCDInfo lcd = new LCDInfo(odo, usp, lsL, lsR);
      Localizer localizer = new Localizer(odo, lsL, lsR, usp);
      GridSnapper gs = new GridSnapper(odo, true);
      BlockFinder bf = new BlockFinder(robot, odo, usp);
      
      // IT IS ASSUMED THAT THE CLAW STARTS OPEN AT 45 DEGREES.
      
      // Setup the claw.
      Claw.init();
      
      // Stop the line detector and grid snapper until needed.
      LineDetector.left.stop();
      LineDetector.right.stop();
      
      // Set initial position to the first gridline intersect (1' x 1'), facing North.
      odo.setPosition(new double [] {FOOT, FOOT, 0.0}, new boolean [] {true, true, true});
      
      // =========================================
      // +-+-+-+  BEGIN MAIN PROGRAM FLOW  +-+-+-+
      // =========================================
      
      
      // Initialize robot - localization and setup.
      init(odo, localizer, gs, usp);
      
      // Run checkAndGo infinitely. This incorporates the search pattern, pallet sweeping,
      // going to the dropoff zone, and stacking the pallets, in an endless loop.
      for (int i = 0; i < 999; i++) {
         int nav = Navigation.checkAndGo(odo, usp, bf);
         
         // If path is blocked, turn right and continue search
         if (nav == -1)
            Navigation.turnRelative(odo, 90);
         
         // If the robot has a pallet, it goes to the tile adjacent to the dropoff zone,
         // localizes, and the stacks the pallet appropriately. Even pallets go on the ground,
         // and odd pallets go above the evens into stacks of 2.
         if (nav == 2) {
            localizer.gridSnapTo((Navigation.DROPOFF_X - 1)*FOOT, (Navigation.DROPOFF_Y)*FOOT, 0);
            
            // Maneuver to line up with the stacking area
            Navigation.turnTo(odo, 135);
            Navigation.goForward(odo, 21.55);
            Navigation.turnTo(odo, 45);
            
            // Lift high for odd pallets to be stacked
            if (stackCount % 2 == 1)
               Claw.liftHigh();
            
            Navigation.goForward(odo, 21.55);
            
            // drop gently if we're stacking a second pallet
            if (stackCount % 2 == 0)
               Claw.release();
            else
               Claw.stack();
            
            stackCount++;
            
            // back up
            Navigation.goForward(odo, -20);
            
            pallet = false;
            
            Beeper.major();
            
            // go back to an adjacent point, face away, and restart the search.
            Navigation.goToPoint(odo, usp, (Navigation.DROPOFF_X - 1) * FOOT, (Navigation.DROPOFF_Y) * FOOT);
            
            Navigation.turnRelative(odo, -90);
         }
         
      }
      
   }
   
   /**
    * Initialize the robot by localizing on the first grid intersection.
    * 
    * @param odo The robot's odometer
    * @param lc The robot's localizer
    * @param gs The robot's grid snapper
    * @param usp The robot's ultrasonic poller
    */
   private static void init(Odometer odo, Localizer lc, GridSnapper gs, USPoller usp) {
      
      // localize to the origin, via US and then LS localization routines.
      gs.disable();
      lc.doUSLocalization();
      lc.gridSnapTo(FOOT, FOOT, 0.0);
      gs.enable();
      
      odo.setPosition(new double [] {FOOT, FOOT, 0.0}, new boolean [] {true, true, true});
      
      Beeper.major();
   }
   
}